vector<FrenetPath> WorldModel::calc_frenet_paths(const PomdpState &state, const Path &ref_path, double curr_speed, double curr_acc,
                                                 double curr_d, double curr_d_d, double curr_d_dd, double s0, double MIN_ROAD_W, double MAX_ROAD_W,
                                                 double DW, double MIN_T, double MAX_T, double DT, double TARGET_SPEED, double D_SPEED, double N_S_SAMPLE, double TIME_STEP)
{
    vector<FrenetPath> frent_paths;
    
    for (double di = MIN_ROAD_W; di < MAX_ROAD_W + DW; di += DW)
    {
        for (double Ti = MIN_T; Ti < MAX_T + DT; Ti += DT)
        {
            FrenetPath target_path;
            QuinticPolynomial lat_qp(curr_d, curr_d_d, curr_d_dd, di, 0.0, 0.0, Ti);

            for (double t = 0.0; t < Ti + TIME_STEP; t += TIME_STEP)
            {
                target_path.t.push_back(t);
            }

            for (double t = target_path.t[0]; t <= target_path.t.back(); t += TIME_STEP)
            {
                target_path.d.push_back(lat_qp.calc_point(t));
                target_path.d_d.push_back(lat_qp.calc_first_derivative(t));
                target_path.d_dd.push_back(lat_qp.calc_second_derivative(t));
                target_path.d_ddd.push_back(lat_qp.calc_third_derivative(t));
            }

            for (double tv = TARGET_SPEED - D_SPEED * N_S_SAMPLE; tv < TARGET_SPEED + D_SPEED; tv += D_SPEED)
            {
                FrenetPath ntfp = target_path;
                QuarticPolynomial lon_qp(s0, curr_speed, curr_acc, tv, 0.0, Ti);

                for (double t = target_path.t[0]; t <= target_path.t.back(); t += TIME_STEP)
                {
                    ntfp.s.push_back(lon_qp.calc_point(t));
                    ntfp.s_d.push_back(lon_qp.calc_first_derivative(t));
                    ntfp.s_dd.push_back(lon_qp.calc_second_derivative(t));
                    ntfp.s_ddd.push_back(lon_qp.calc_third_derivative(t));
                }

                // Calculate the cost
                double Jp = 0.0, Js = 0.0;
                for (auto i : ntfp.d_ddd)
                    Jp += i * i;
                for (auto i : ntfp.s_ddd)
                    Js += i * i;

                double K_J = 0.1, K_T = 0.1, K_D = 1.0, K_LAT = 1.0, K_LON = 1.0;
                double ds = (TARGET_SPEED - ntfp.s_d[-1]) * (TARGET_SPEED - ntfp.s_d[-1]);
                ntfp.cd = K_J * Jp + K_T * Ti + K_D * ntfp.d[-1] * ntfp.d[-1];
                ntfp.cv = K_J * Js + K_T * Ti + K_D * ds;
                ntfp.cf = K_LAT * ntfp.cd + K_LON * ntfp.cv;

                frent_paths.push_back(ntfp);
            }
        }
    }
    return frent_paths;
}

void WorldModel::calc_global_paths(vector<FrenetPath> &fplist, const Path &ref_path)
{
    auto start_time = Time::now();
    CONTEXTPOMDP_FRENET2CARTESIAN_TIMES++;
    int round = -1;
    for (auto &p : fplist)
    // auto &p = fplist[0];
    {
        round++;
        for (int i = 0; i < p.s.size(); i++)
        {
            // AD_LERROR(DEBUG_GLOBAL) << "------------------------- " << i << " ---------------------------";
            double rs = p.s[i];
            double steps = rs / ref_path.path_step;
            int pre_index = int(steps);
            double rx, ry, rtheta, rkappa, rdkappa;
            if (pre_index >= ref_path.size())
            {
                if (ref_path.size() <= 1 || p.x.size() >= 2)
                    break;
                // AD_LERROR(calc_global_paths) << "enlarge the path, target_index: " << pre_index << ", path size: " << ref_path.size();
                int pre_ = ref_path.size() - 2, next_ = ref_path.size() - 1;
                int times = pre_index - pre_;
                rx = times * (ref_path[next_].x - ref_path[pre_].x) + ref_path[pre_].x;
                ry = times * (ref_path[next_].y - ref_path[pre_].y) + ref_path[pre_].y;
                rtheta = 0.5 * (ref_path.headings[pre_] + ref_path.headings[next_]);
                rkappa = 0;
                rdkappa = 0;
            }
            else
            {
                int next_index = pre_index + 1;
                if (next_index >= ref_path.size())
                    next_index = ref_path.size() - 1;

                double coeff = steps - pre_index;

                rx = (1 - coeff) * ref_path[pre_index].x + coeff * ref_path[next_index].x;
                ry = (1 - coeff) * ref_path[pre_index].y + coeff * ref_path[next_index].y;
                rtheta = (1 - coeff) * ref_path.headings[pre_index] + coeff * ref_path.headings[next_index];
                rkappa = (1 - coeff) * ref_path.kappas[pre_index] + coeff * ref_path.kappas[next_index];
                rdkappa = (1 - coeff) * ref_path.dkappas[pre_index] + coeff * ref_path.dkappas[next_index];
                if (i == 0 && round == 0)
                {
                    // AD_LERROR(FRENET2CARTESIAN) << "coeff: " << coeff << ", pre_index: " << pre_index << ", next_index: " << next_index;
                    // AD_LERROR(FRENET2CARTESIAN) << "rs: " << rs << ", rx: " << rx << ", ry: " << ry << ", heading: " << rtheta << ", kappa: " << rkappa << ", dkappas: " << rdkappa;
                }
            }

            const double cos_theta_r = cos(rtheta);
            const double sin_theta_r = sin(rtheta);

            p.x.push_back(rx - sin_theta_r * p.d[i]);
            p.y.push_back(ry + cos_theta_r * p.d[i]);

            const double one_minus_kappa_r_d = 1 - rkappa * p.d[i];

            const double tan_delta_theta = p.d_d[i] / p.s_d[i] / one_minus_kappa_r_d;
            const double delta_theta = atan2(p.d_d[i] / p.s_d[i], one_minus_kappa_r_d);
            const double cos_delta_theta = cos(delta_theta);

            p.theta.push_back(NormalizeAngle(delta_theta + rtheta));

            const double kappa_r_d_prime = rdkappa * p.d[i] + rkappa * p.d_d[i] / p.s_d[i];

            double d_ss = (p.d_dd[i] - p.s_dd[i] * (p.d_d[i] / p.s_d[i])) / (p.s_d[i] * p.s_d[i]);

            p.kappa.push_back((((d_ss + kappa_r_d_prime * tan_delta_theta) * cos_delta_theta * cos_delta_theta) /
                                   (one_minus_kappa_r_d) +
                               rkappa) *
                              cos_delta_theta / (one_minus_kappa_r_d));

            p.v.push_back(sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d * p.s_d[i] * p.s_d[i] + p.d_d[i] * p.d_d[i]));

            const double delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * p.kappa.back() - rkappa;

            p.a.push_back(p.s_dd[i] * one_minus_kappa_r_d / cos_delta_theta +
                          p.s_d[i] * p.s_d[i] / cos_delta_theta * (p.d_d[i] / p.s_d[i] * delta_theta_prime - kappa_r_d_prime));
        }
    }
    CONTEXTPOMDP_FRENET2CARTESIAN += Globals::ElapsedTime(start_time);
}

FrenetCoords cartesian_to_frenet(
	const double rs, const double rx, const double ry, const double rtheta,
	const double rkappa, const double rdkappa, const double x, const double y,
	const double v, const double a, const double theta, const double kappa)
{
	FrenetCoords coords;

	// Step 1: Calculate the car's position along and from the reference line
	const double dx = x - rx;
	const double dy = y - ry;

	const double cos_theta_r = cos(rtheta);
	const double sin_theta_r = sin(rtheta);

	const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;
	coords.s = rs;
	coords.d = copysign(sqrt(dx * dx + dy * dy), cross_rd_nd);

	// Step 2: Calculate the car's velocity along and from the reference line
	const double delta_theta = theta - rtheta;
	const double tan_delta_theta = tan(delta_theta);
	const double cos_delta_theta = cos(delta_theta);
	const double sin_delta_theta = sin(delta_theta);
	const double one_minus_kappa_r_d = 1 - rkappa * coords.d;

	coords.d_d = v * sin_delta_theta;
	coords.s_d = v * cos_delta_theta / one_minus_kappa_r_d;
	double d_s = one_minus_kappa_r_d * tan_delta_theta;

	// Step 3: Calculate the car's acc along and from the reference line
	const double kappa_r_d_prime = rdkappa * coords.d + rkappa * d_s;

	double d_ss = -kappa_r_d_prime * tan_delta_theta + one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *
														   (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);

	double delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;

	coords.s_dd = (a * cos_delta_theta - coords.s_d * coords.s_d * (d_s * delta_theta_prime - kappa_r_d_prime)) / one_minus_kappa_r_d;

	coords.d_dd = d_ss * coords.s_d * coords.s_d + coords.s_dd * d_s;
	
	return coords;
}

Path Path::Interpolate(double max_len) const
{
	auto &path = *this;
	Path p;

	const double step = path_step;
	double t = 0, ti = 0;
	for (int i = 0; i < path.size() - 1; i++)
	{
		double d = COORD::EuclideanDistance(path[i], path[i + 1]);
		// assert(d < 1001.0);
		double dx = (path[i + 1].x - path[i].x) / d;
		double dy = (path[i + 1].y - path[i].y) / d;
		double sx = path[i].x;
		double sy = path[i].y;
		while (t < ti + d)
		{
			double u = t - ti;
			double nx = sx + dx * u;
			double ny = sy + dy * u;

			if (p.size() == 0 || nx != p.back().x || ny != p.back().y)
				p.push_back(COORD(nx, ny));

			t += step;

			if (p.size() * path_step >= max_len)
				break;
		}

		ti += d;
	}
	p.push_back(path[path.size() - 1]);
	p.coeffs = path.coeffs;
	return p;
}

bool Path::ComputePathProfile_formula()
{
	headings.clear();
	accumulated_s.clear();
	kappas.clear();
	dkappas.clear();

	auto& xy_points = *this;
	if (xy_points.size() < 2)
	{
		return false;
	}
	vector<double> dxs;
	vector<double> dys;

	double a = xy_points.coeffs[3];
	double b = xy_points.coeffs[2];
	double c = xy_points.coeffs[1];

	// Get finite difference approximated dx and dy for heading and kappa
	// calculation
	size_t points_size = xy_points.size();
	for (size_t i = 0; i < points_size; ++i)
	{
		double x_delta = 0.0;
		double y_delta = 0.0;
		if (i == 0)
		{
			x_delta = (xy_points[i + 1].x - xy_points[i].x);
			y_delta = (xy_points[i + 1].y - xy_points[i].y);
		}
		else if (i == points_size - 1)
		{
			x_delta = (xy_points[i].x - xy_points[i - 1].x);
			y_delta = (xy_points[i].y - xy_points[i - 1].y);
		}
		else
		{
			x_delta = 0.5 * (xy_points[i + 1].x - xy_points[i - 1].x);
			y_delta = 0.5 * (xy_points[i + 1].y - xy_points[i - 1].y);
		}
		dxs.push_back(x_delta);
		dys.push_back(y_delta);
	}

	// Get linear interpolated s for dkappa calculation
	double distance = 0.0;
	accumulated_s.push_back(distance);
	double fx = xy_points[0].x;
	double fy = xy_points[0].y;
	double nx = 0.0;
	double ny = 0.0;
	for (size_t i = 1; i < points_size; ++i)
	{
		nx = xy_points[i].x;
		ny = xy_points[i].y;
		double end_segment_s = sqrt((fx - nx) * (fx - nx) + (fy - ny) * (fy - ny));
		accumulated_s.push_back(end_segment_s + distance);
		distance += end_segment_s;
		fx = nx;
		fy = ny;
	}

	for (size_t i = 0; i < points_size; ++i)
	{
		double x = xy_points[i].x;
		double y_x = 3 * a * x * x + 2 * b * x + c;
		double y_xx = 6 * a * x + 2 * b;
		headings.push_back(atan(y_x));
		kappas.push_back(y_xx / pow((1 + y_x * y_x), 1.5));
	}

	for (size_t i = 0; i < points_size; ++i)
	{
		double dkappa = 0.0;
		if (i == 0)
		{
			dkappa = (kappas[i + 1] - kappas[i]) /
					 (accumulated_s[i + 1] - accumulated_s[i]);
		}
		else if (i == points_size - 1)
		{
			dkappa = (kappas[i] - kappas[i - 1]) /
					 (accumulated_s[i] - accumulated_s[i - 1]);
		}
		else
		{
			dkappa = (kappas[i + 1] - kappas[i - 1]) /
					 (accumulated_s[i + 1] - accumulated_s[i - 1]);
		}
		dkappas.push_back(dkappa);
	}

	return true;
}

void WorldModel::process_store_lane_line(const MapLaneptr &curr_lane_ptr)
{
	Path p;
	for (const auto &i : curr_center_line)
		p.push_back(COORD(i.x, i.y));
	p.path_step = ModelParams::DEFAULT_PATH_STEP;
	p = p.Smoothing();
	p = p.Interpolate();
	bool pathstate = p.ComputePathProfile_formula();
	if (pathstate == false)
		AD_LERROR("Invalid path state");
	this->lane_id_map_smoothing_path[curr_lane_id] = p;
}


int main()
{
    CONTEXTPOMDP_NEAREST_TIMES++;
    auto start_time = Time::now();
    int pre_path_index = curr_path.Nearest(car_pos);
    CONTEXTPOMDP_NEAREST += Globals::ElapsedTime(start_time);

    if (pre_path_index == curr_path.size() - 1)
        pre_path_index = curr_path.size() - 2;

    int next_path_index = pre_path_index + 1;

    double path_coeff = ((car_pos.x - curr_path[pre_path_index].x) * (curr_path[next_path_index].x - curr_path[pre_path_index].x) + (car_pos.y - curr_path[pre_path_index].y) * (curr_path[next_path_index].y - curr_path[pre_path_index].y)) / curr_path.path_step / curr_path.path_step;

    double accumulated_s = (1 - path_coeff) * curr_path.accumulated_s[pre_path_index] + path_coeff * curr_path.accumulated_s[next_path_index];
    double steps = accumulated_s / curr_path.path_step;
    pre_path_index = int(steps);
    if (pre_path_index >= curr_path.size() - 1)
        pre_path_index = curr_path.size() - 2;
    next_path_index = pre_path_index + 1;

    path_coeff = steps - pre_path_index;

    double rx = (1 - path_coeff) * curr_path[pre_path_index].x + path_coeff * curr_path[next_path_index].x;
    double ry = (1 - path_coeff) * curr_path[pre_path_index].y + path_coeff * curr_path[next_path_index].y;
    double heading = (1 - path_coeff) * curr_path.headings[pre_path_index] + path_coeff * curr_path.headings[next_path_index];
    double kappa = (1 - path_coeff) * curr_path.kappas[pre_path_index] + path_coeff * curr_path.kappas[next_path_index];
    double dkappas = (1 - path_coeff) * curr_path.dkappas[pre_path_index] + path_coeff * curr_path.dkappas[next_path_index];

    static constexpr double kEpsilon = 1e-6;
    if (abs(state.car.speed) < kEpsilon)
        state.car.curvature = 0.0;
    else
        state.car.curvature = state.car.yaw_rate / state.car.speed;

    CONTEXTPOMDP_CARTESIAN2FRENET_TIMES++;
    start_time = Time::now();
    FrenetCoords F_coord = cartesian_to_frenet(accumulated_s, rx, ry, heading, kappa, dkappas, car_pos.x, car_pos.y,
                                               state.car.speed, state.car.acc, state.car.heading_dir, state.car.curvature);

    CONTEXTPOMDP_CARTESIAN2FRENET += Globals::ElapsedTime(start_time);

    double d = F_coord.d, d_d = F_coord.d_d, d_dd = F_coord.d_dd, s0 = F_coord.s, s_speed = F_coord.s_d, s_acc = F_coord.s_dd;

    double MIN_T = 3.0, MAX_T = 5.0, Sample_time_difference = 0.2;
    double width = target_lane_ptr->GetWidth(Point3D_t(state.car.pos.x, state.car.pos.y), HDMapUtil::GetInstance()->GetCenterLaneLine(target_lane_ptr));

    double discount = 1.0;
    if (turn_left)
        discount = ((width + curr_lane_width) / 2 - d) / ((width + curr_lane_width) / 2) / 1.2;
    else
        discount = ((width + curr_lane_width) / 2 + d) / ((width + curr_lane_width) / 2) / 1.2;

    vector<FrenetPath> target_paths;
    MIN_T *= discount;
    MAX_T *= discount;
    double DT = (MAX_T - MIN_T) / 2.0;
    double target_speed = min(ModelParams::VEL_MAX, MIN_T * ModelParams::MAX_ACC / 2.0 + state.car.speed);

    // AD_LERROR(FRENTE) << "Input parameters: " << "s0: " << s0 << ", s_speed: " << s_speed << ", s_acc:" << s_acc << ", d: " << d << ", d_d: " << d_d << ", d_dd: " << d_dd << ", MIN_WIDTH: " << (curr_lane_width / 2) + (width / 5) << ", MAX_WIDTH: " << (curr_lane_width / 2) + width - (width / 5) << ", DELTA_WIDTH: " << width / 5 << ", MIN_T: " << MIN_T << ", MAX_T: " << MAX_T << ", DT: " << DT << ", TARGET_SPEED: " << target_speed << ", turn left: " << turn_left;
    // AD_LERROR(CAR_INFO) << "s0: " << accumulated_s << ", rx: " << rx << ", ry: " << ry << ", heading: " << heading << ", kappa: " << kappa << ", dkappas: " << dkappas << ", car_pos.x: " << car_pos.x << ", car_pos.y: " << car_pos.y << ", speed: " << state.car.speed << ", acc: " << state.car.acc << ", heading: " << state.car.heading_dir << ", curvature: " << state.car.curvature;

    if (turn_left)
        target_paths = world_model.calc_frenet_paths(state, curr_path, s_speed, s_acc, d, d_d, d_dd, s0, (curr_lane_width / 2) + (2 * width / 5), (curr_lane_width + width) / 2, width / 10, MIN_T, MAX_T, DT, target_speed, 0.5, 0, Sample_time_difference);
    else
        target_paths = world_model.calc_frenet_paths(state, curr_path, s_speed, s_acc, d, d_d, d_dd, s0, -(curr_lane_width + width) / 2, -(curr_lane_width / 2) - (2 * width / 5), width / 10, MIN_T, MAX_T, DT, target_speed, 0.5, 0, Sample_time_difference);

    // Transform frent coordinates to Cartesian coordinates
    world_model.calc_global_paths(target_paths, curr_path);

    // Filter out the invalid paths
    if (!calculate_bound)
        world_model.check_paths(state, target_paths, state.car);

    // Find the path that has the minimum cost
    FrenetPath target_path;
    if (target_paths.size() == 0)
    {
        // No available paths, so keep the current lane
        AD_LERROR(DEBUG_IDM) << "target_paths.size() == 0";
        // use IDM to calculate the acc
        double acc;
        if (calculate_bound)
            acc = world_model.IDM(state, 0, true);
        else
            acc = world_model.IDM(state);

        // use the Pure Pursuit to calculate the steering
        double steering = world_model.GetSteerToPath(state.car);

        // use the bicycle model to update the state
        world_model.RobStep(state.car, steering, rNum);
        world_model.RobVelStep(state.car, acc, rNum);
    }
    else
    {
        double min_cost = ModelParams::MAX_VALUE;
        for (auto &path : target_paths)
        {
            if (path.cf < min_cost)
            {
                target_path = path;
                min_cost = path.cf;
            }
        }
        // update the state of the car
        double time_steps = delta_time / Sample_time_difference;
        int pre_index, next_index;
        double coeff;
        if (time_steps >= target_path.x.size())
        {
            pre_index = target_path.x.size() - 2;
            next_index = target_path.x.size() - 1;
            coeff = time_steps - pre_index;
        }
        else
        {
            pre_index = int(time_steps);
            next_index = pre_index + 1;
            coeff = time_steps - pre_index;
        }
        state.car.pos = COORD(target_path.x[pre_index], target_path.y[pre_index]) * (1 - coeff) + COORD(target_path.x[next_index], target_path.y[next_index]) * coeff;
        state.car.heading_dir = (1 - coeff) * target_path.theta[pre_index] + coeff * target_path.theta[next_index];
        state.car.speed = (1 - coeff) * target_path.v[pre_index] + coeff * target_path.v[next_index];
        state.car.vel = COORD(state.car.speed * cos(state.car.heading_dir), state.car.speed * sin(state.car.heading_dir));
        state.car.acc = (1 - coeff) * target_path.a[pre_index] + coeff * target_path.a[next_index];
        state.car.acc_vec = COORD(state.car.acc * cos(state.car.heading_dir), state.car.acc * sin(state.car.heading_dir));
        state.car.curvature = (1 - coeff) * target_path.kappa[pre_index] + coeff * target_path.kappa[next_index];
        state.car.yaw_rate = state.car.curvature * state.car.speed;
    }
}